/*============================================================================*/
/*                                                                            */
/*  FILE       : filters.h                                                    */
/*  CONTENTS   : Contains support for motion cueing filters.                  */
/*----------------------------------------------------------------------------*/
/*    $Archive:: /Software/nm_appln/FILTERS.H                                $*/
/*   $Revision:: 386                                                         $*/
/*----------------------------------------------------------------------------*/
/*  VISIBILITY : Public                                                       */
/*               Copyright (c) 1997 Motionbase PLC                            */
/*               All rights reserved.                                         */
/*----------------------------------------------------------------------------*/
/*  HISTORY    :                                                              */
/*                                                                            */
/*    Date     Author    Description                                          */
/*  15/05/96     RND     Created.                                             */
/*  26/11/97     KG      Converted to Motionbase format.                      */
/*  10/03/98     KG      Error codes moved to err_code.h                      */
/*  15/03/98     RND     Continuous to discrete conversion moved to           */
/*                       NextMove code.                                       */
/*  06/08/98     KG      getFilterInput added.                                */
/*                       Analogue input values to filters normalised +/-1.    */
/*  15/09/98     RND     Filters changed to have first order numerators and   */
/*                       third order denomenators.                            */
/*  27/10/98     RND     Added saturation of filter inputs.                   */
/*                       Added state limiting of filter outputs.              */
/*  20/01/99     RND     Revised the implementation of the filters to         */
/*                       eliminate instability problems.                      */
/*  04/02/99     RND     Added motion cueing features.                        */
/*  10/05/99     KG      Added:                                               */
/*============================================================================*/
#ifndef _FILTERS_H_
#define _FILTERS_H_

#ifdef __cplusplus
extern "C" {
#endif

/*============================================================================*/
/* INCLUDES                                                                   */
/*============================================================================*/
#include "platform.h"                  /* _lastPlatformFunction, ...          */

/*============================================================================*/
/* MACROS                                                                     */
/*============================================================================*/
#define cnNUMBER_OF_FILTER_BLOCKS 8
#define cnNUMBER_OF_HOST_DATA_LATCHES 6
#define cnNUMBER_OF_ANALOGUE_SOURCES 6

/*----------------------------------------------------------------------------*/
/* Define state limiting bits.                                                */
/*----------------------------------------------------------------------------*/
#define FILTER_LIM_ACCELERATION 0x08
#define FILTER_LIM_VELOCITY     0x04
#define FILTER_LIM_MAX_POSITION 0x02
#define FILTER_LIM_MIN_POSITION 0x01

/*============================================================================*/
/* TYPEDEFS                                                                   */
/*============================================================================*/
/*----------------------------------------------------------------------------*/
/* Enumerate the signal sources that can be connected to the filter block     */
/* inputs.                                                                    */
/*----------------------------------------------------------------------------*/
typedef enum {
  fiNONE = 0,
  fiHOST_DATA_LATCH_0,
  fiHOST_DATA_LATCH_1,
  fiHOST_DATA_LATCH_2,
  fiHOST_DATA_LATCH_3,
  fiHOST_DATA_LATCH_4,
  fiHOST_DATA_LATCH_5,
  fiANALOGUE_0,
  fiANALOGUE_1,
  fiANALOGUE_2,
  fiANALOGUE_3,
  fiANALOGUE_4,
  fiANALOGUE_5,
  fiLAST
} TFilterBlockInput;

/*----------------------------------------------------------------------------*/
/* Enumerate the kinematics inputs.                                           */
/*----------------------------------------------------------------------------*/
typedef enum {
  kiNONE = 0,
  ki0,
  ki1,
  ki2,
  ki3,
  ki4,
  ki5,
  kiLAST
} TKinematicsInput;

/*----------------------------------------------------------------------------*/
/* Enumerate filter functions                                                 */
/*----------------------------------------------------------------------------*/
enum filterFunctions {
  _getFilterBlockInput=_lastPlatformFunction,
  _getMotionCueing,
  _setFilterBlock,
  _setMotionCueing,
  _lastFilterFunction
};

/*----------------------------------------------------------------------------*/
/* Define a structure to hold the parameters of a 1-pole, 1-zero continuous-  */
/* time filter, i.e.                                                          */
/*                                                                            */
/*            n1*s + n0                                                       */
/*     g(s) = ---------                                                       */
/*            d1*s + d0                                                       */
/*                                                                            */
/* where d0=1.                                                                */
/*----------------------------------------------------------------------------*/
typedef struct {
  /*--------------------------------------------------------------------------*/
  /* The filter coefficients.                                                 */
  /*--------------------------------------------------------------------------*/
  float n1;                            /* Numerator s^1 coefficient.          */
  float n0;                            /* Numerator s^0 coefficient.          */
  float d1;                            /* Denomenator s^1 coefficient.        */
} TContinuousTimeFilter1p1z;

/*----------------------------------------------------------------------------*/
/* Define a structure to hold the parameters of a 2-pole continuous-time      */
/* filter, i.e.                                                               */
/*                      1                                                     */
/*     g(s) = K ------------------                                            */
/*              d2*s^2 + d1*s + d0                                            */
/*                                                                            */
/* where d0=1.                                                                */
/*----------------------------------------------------------------------------*/
typedef struct {
  /*--------------------------------------------------------------------------*/
  /* The filter coefficients.                                                 */
  /*--------------------------------------------------------------------------*/
  float K;                             /* Gain.                               */
  float d2;                            /* Denomenator s^2 coefficient.        */
  float d1;                            /* Denomenator s^1 coefficient.        */
} TContinuousTimeFilter2p0z;

/*----------------------------------------------------------------------------*/
/* Define the parameters of the zero-going response elimination.              */
/*----------------------------------------------------------------------------*/
typedef struct {
  INT  on;                    /* True if ZRE is enabled.                      */
  FLOAT upperFreq;            /* The frequency above which ZRE will not be    */
                              /* applied.                                     */
} TZRE;

/*----------------------------------------------------------------------------*/
/* Define the parameters of a state limiter.                                  */
/*----------------------------------------------------------------------------*/
typedef struct {
  INT   on;                   /* True if state limiting is enabled.           */
  FLOAT yMax;                 /* Upper output limit.                          */
  FLOAT yMin;                 /* Lower output limit.                          */
  FLOAT vMax;                 /* Output velocity limit.                       */
  FLOAT aMax;                 /* Output acceleration limit.                   */
  FLOAT aMaxForOutLim;        /* Output acceleration limit for ouput limiting.*/
} TStateLimiter;

/*----------------------------------------------------------------------------*/
/* Define a structure to hold the parameters of a filter block.               */
/*----------------------------------------------------------------------------*/
typedef struct {
  /*--------------------------------------------------------------------------*/
  /* The signal source that is connected to the filter input.                 */
  /*--------------------------------------------------------------------------*/
  TFilterBlockInput input;
  /*--------------------------------------------------------------------------*/
  /* The kinematics input to which the filter output is connected.            */
  /*--------------------------------------------------------------------------*/
  TKinematicsInput output;
  /*--------------------------------------------------------------------------*/
  /* The gain applied to the input signal (mainly used for scaling the        */
  /* analogue input signals to represent meaningful units).                   */
  /*--------------------------------------------------------------------------*/
  FLOAT Ki;
  /*--------------------------------------------------------------------------*/
  /* The input saturation limits.                                             */
  /*--------------------------------------------------------------------------*/
  FLOAT xMax;                         /* Upper saturation limit.              */
  FLOAT xMin;                         /* Lower saturation limit.              */
  /*--------------------------------------------------------------------------*/
  /* The parameters of the zero-going response elimination.                   */
  /*--------------------------------------------------------------------------*/
  TZRE ZRE;
  /*--------------------------------------------------------------------------*/
  /* The adaption gains.                                                      */
  /*--------------------------------------------------------------------------*/
  FLOAT Ka[cnMAX_NUMBER_OF_ACTUATORS];
  /*--------------------------------------------------------------------------*/
  /* The two filter stages.                                                   */
  /*--------------------------------------------------------------------------*/
  TContinuousTimeFilter1p1z filterA;
  TContinuousTimeFilter2p0z filterB;
  /*--------------------------------------------------------------------------*/
  /* The filter state limiter.                                                */
  /*--------------------------------------------------------------------------*/
  TStateLimiter stateLimiter;
  /*--------------------------------------------------------------------------*/
  /* The gain applied to the output of the filter block (mainly used for      */
  /* converting the output to the units used by the kinematics block).        */
  /*--------------------------------------------------------------------------*/
  FLOAT Ko;
} TContinuousTimeFilterBlock;

/*============================================================================*/
/* FUNCTION PROTOTYPES                                                        */
/*============================================================================*/
/*----------------------------------------------------------------------------*/
/* getFilterBlockInput                                                        */
/* Returns the current input value of a specified filter block.               */
/*                                                                            */
/* Args:                                                                      */
/* blockNo - the filter block index.                                          */
/* pValue  - where to store the data                                          */
/*                                                                            */
/* Return:                                                                    */
/* erSUCCESS                                                                  */
/* erFILTER_NUMBER_OUT_OF_RANGE                                               */
/*----------------------------------------------------------------------------*/
CCONV INT getFilterBlockInput (UINT blockNo, FLOAT FAR* pValue);

/*----------------------------------------------------------------------------*/
/* getMotionCueing                                                            */
/* Returns the flags which determine whether the motion cueing algorithms are */
/* enabled.                                                                   */
/*                                                                            */
/* Args:                                                                      */
/* motionCueingOn   - zero: motion cueing is disabled,                        */
/*                    non-zero: motion cueing is enabled.                     */
/* specificForcesOn - zero: the translational accelerations do not include    */
/*                    the acceleration due to gravity,                        */
/*                    non-zero: the translational accelerations do include    */
/*                    the acceleration due to gravity.                        */
/* adaptionOn       - zero: adaption of the cueing filter gains is disabled.  */
/*                    non-zero: adaption of the gains of the cueing filters   */
/*                    is enabled.                                             */
/*                                                                            */
/* Return:                                                                    */
/* erSUCCESS                                                                  */
/*----------------------------------------------------------------------------*/
CCONV INT getMotionCueing(BYTE* motionCueingOn,
                          BYTE* specificForcesOn,
                          BYTE* adaptionOn);

/*----------------------------------------------------------------------------*/
/* setFilterBlock                                                             */
/* Sets the parameters of a filter block.                                     */
/*                                                                            */
/* Args:                                                                      */
/* blockNo      - the filter block index.                                     */
/* pcfb         - pointer to the continuous-time filter block structure that  */
/*                contains the filter block parameters.                       */
/* T            - the sample interval. This must be equal to the state-       */
/*                machine period.                                             */
/*                                                                            */
/* Return:                                                                    */
/* erSUCCESS                                                                  */
/* erFILTER_NUMBER_OUT_OF_RANGE                                               */
/* erFILTER_INPUT_OUT_OF_RANGE                                                */
/* erKINEMATICS_INPUT_OUT_OF_RANGE                                            */
/* erUNSTABLE_FILTER                                                          */
/* erVALUE_OUT_OF_RANGE                                                       */
/*----------------------------------------------------------------------------*/
CCONV INT setFilterBlock(UINT blockNo,
                         TContinuousTimeFilterBlock FAR* pcfb,
                         FLOAT T);

/*----------------------------------------------------------------------------*/
/* setMotionCueing                                                            */
/* Enables or disables the motion cueing algorithms, and sets whether the     */
/* translational accelerations are to be interpreted as specific forces (i.e. */
/* whether they include the acceleration due to gravity). The setting for the */
/* specific forces is only relevant if the motion cueing algorithms are       */
/* enabled.                                                                   */
/*                                                                            */
/* Args:                                                                      */
/* motionCueingOn   - zero: disables motion cueing,                           */
/*                    non-zero: enables motion cueing.                        */
/* specificForcesOn - zero: the translational accelerations do not include    */
/*                    the acceleration due to gravity,                        */
/*                    non-zero: the translational accelerations do include    */
/*                    the acceleration due to gravity.                        */
/* adaptionOn       - zero: disables adaption of the cueing filter gains.     */
/*                    non-zero: the gains of the cueing filters are altered   */
/*                    adaptively.                                             */
/*                                                                            */
/* Return:                                                                    */
/* erSUCCESS                                                                  */
/*----------------------------------------------------------------------------*/
CCONV INT setMotionCueing(BYTE motionCueingOn,
                          BYTE specificForcesOn,
                          BYTE adaptionOn);

#ifdef __cplusplus
} /* extern "C" */
#endif

#endif /* _FILTERS_H_ */
/*============================================================================*/
/* END OF FILE                                                                */
/*============================================================================*/
